/**
 * @file /src/biotacgui.cpp
 * @author Tomonori Yamamoto <tomonori.yamamoto@syntouchllc.com>
 *         (originally written by Daniel Stonier)
 * @date December 02, 2011
 * @brief Implementation for the BioTac QT GUI.
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QMessageBox>
#include <QMap>
#include <QString>
#include <iostream>
#include "st_biotac_gui/biotacgui.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace syntouch_biotac_gui {

const int BioTacGui::width_ = 45;
const int BioTacGui::height_ = 45;
const int BioTacGui::font_size_1_ = 24;
const int BioTacGui::font_size_2_ = 22;
const int BioTacGui::num_sensing_electrodes_ = 19;
const int BioTacGui::num_excitation_electrodes_ = 4;
const BiotacData* BioTacGui::data_ptr_(new BiotacData [5]);

enum BiotacId { FF, MF, RF, LF, TH };
typedef QMap<QString, BiotacId> QStringToEnumMap;
QStringToEnumMap map;

/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/

BioTacGui::BioTacGui(int argc, char** argv, QWidget *parent)
  : qnode_(argc, argv),
    ui_(new Ui::BioTacGuiWindow)
{
  ui_->setupUi(this);

  BioTacGui::bt_index_ = 0;
  BioTacGui::data_ptr_ = qnode_.getData();

  // mapping from Qstring to enum
  map.insert("FF", FF);
  map.insert("MF", MF);
  map.insert("RF", RF);
  map.insert("LF", LF);
  map.insert("TH", TH);

  // Qt signals/slots
  QObject::connect(&qnode_, SIGNAL(dataUpdated()), this, SLOT(updateGui()));
  QObject::connect(&qnode_, SIGNAL(rosShutdown()), this, SLOT(closeGui()));
}

BioTacGui::~BioTacGui()
{
}

void BioTacGui::paintEvent(QPaintEvent *event)
{
  QPainter painter(this);
  
  int i;
  
  qreal tmp_x, tmp_y, tmp_w, tmp_h;
  qreal factor = 17.5;
  qreal offset_x1 = 150.0, offset_y1 = -50.0;
  qreal offset_x2 = offset_x1 + 12.5, offset_y2 = offset_y1 + 4.0;
  qreal offset_x3 = offset_x1 + 4.5, offset_y3 = offset_y1 + 4.0;
  qreal offset_x4 = offset_x1 + 3.5, offset_y4 = offset_y1 + 4.0;
  
  // impedance sensing electrodes
  // x and y positions of the electrodes from Gary's original CAD file
  sensing_electrode_[0].setRect(7.58, 6.45, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[1].setRect(11.28, 3.65, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[2].setRect(14.78, 3.65, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[3].setRect(16.58, 6.45, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[4].setRect(19.08, 3.65, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[5].setRect(21.98, 6.45, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[6].setRect(4.38, 0.00, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[7].setRect(6.38, 1.95, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[8].setRect(6.38, -1.95, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[9].setRect(8.38, 0.00, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[10].setRect(7.58, -6.45, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[11].setRect(11.28, -3.65, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[12].setRect(14.78, -3.65, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[13].setRect(16.58, -6.45, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[14].setRect(19.08, -3.65, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[15].setRect(21.98, -6.45, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[16].setRect(11.38, 0.00, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[17].setRect(18.38, 0.00, BioTacGui::width_, BioTacGui::height_);
  sensing_electrode_[18].setRect(22.18, 0.00, BioTacGui::width_, BioTacGui::height_);

  // excitation electrodes
  // x and y positions of the electrodes from Gary's original CAD file
  excitation_electrode_[0].setRect(12.48, 6.45, BioTacGui::width_, BioTacGui::height_);
  excitation_electrode_[1].setRect(24.48, 3.75, BioTacGui::width_, BioTacGui::height_);
  excitation_electrode_[2].setRect(24.48, -3.75, BioTacGui::width_, BioTacGui::height_);
  excitation_electrode_[3].setRect(12.48, -6.45, BioTacGui::width_, BioTacGui::height_);
  
  painter.setRenderHint(QPainter::Antialiasing, true);
  painter.setPen(QPen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap));

  // check BioTac ID
  QString biotac_id = ui_->btSelect->currentText();

  switch( map[biotac_id] )
  {
  case FF:
    bt_index_ = 0;
    break;
  case MF:
    bt_index_ = 1;
    break;
  case RF:
    bt_index_ = 2;
    break;
  case LF:
    bt_index_ = 3;
    break;
  case TH:
    bt_index_ = 4;
    break;
  }

  // display electrode data on the LCDs
  ui_->lcdE01->display(data_ptr_[bt_index_].electrodes[0]);
  ui_->lcdE02->display(data_ptr_[bt_index_].electrodes[1]);
  ui_->lcdE03->display(data_ptr_[bt_index_].electrodes[2]);
  ui_->lcdE04->display(data_ptr_[bt_index_].electrodes[3]);
  ui_->lcdE05->display(data_ptr_[bt_index_].electrodes[4]);
  ui_->lcdE06->display(data_ptr_[bt_index_].electrodes[5]);
  ui_->lcdE07->display(data_ptr_[bt_index_].electrodes[6]);
  ui_->lcdE08->display(data_ptr_[bt_index_].electrodes[7]);
  ui_->lcdE09->display(data_ptr_[bt_index_].electrodes[8]);
  ui_->lcdE10->display(data_ptr_[bt_index_].electrodes[9]);
  ui_->lcdE11->display(data_ptr_[bt_index_].electrodes[10]);
  ui_->lcdE12->display(data_ptr_[bt_index_].electrodes[11]);
  ui_->lcdE13->display(data_ptr_[bt_index_].electrodes[12]);
  ui_->lcdE14->display(data_ptr_[bt_index_].electrodes[13]);
  ui_->lcdE15->display(data_ptr_[bt_index_].electrodes[14]);
  ui_->lcdE16->display(data_ptr_[bt_index_].electrodes[15]);
  ui_->lcdE17->display(data_ptr_[bt_index_].electrodes[16]);
  ui_->lcdE18->display(data_ptr_[bt_index_].electrodes[17]);
  ui_->lcdE19->display(data_ptr_[bt_index_].electrodes[18]);
  
  // color of sensing electrodes change based on the sensor readings
  for(i=0; i<BioTacGui::num_sensing_electrodes_; i++)
  {
    sensing_electrode_[i].getRect(&tmp_x, &tmp_y, &tmp_w, &tmp_h);
    sensing_electrode_[i].setRect(tmp_y*factor + offset_x1, tmp_x*factor + offset_y1, tmp_w, tmp_h);
    convertRawToRgb(data_ptr_[bt_index_].electrodes[i]);
    painter.setBrush(QColor(red_, green_, blue_));
    painter.drawEllipse(sensing_electrode_[i]);
    painter.setFont(QFont("Arial", BioTacGui::font_size_1_));

    // adjust x,y position for the electrode numbers
    if (i<9)
    {
      sensing_electrode_[i].setRect(tmp_y*factor + offset_x2, tmp_x*factor + offset_y2, tmp_w, tmp_h);
    }
    else
    {
      sensing_electrode_[i].setRect(tmp_y*factor + offset_x3, tmp_x*factor + offset_y3, tmp_w, tmp_h);
    }

    painter.drawText(sensing_electrode_[i], QString::number(i+1));
  }

  // color of the exitation electrodes are always gray
  for(i=0; i<BioTacGui::num_excitation_electrodes_; i++)
  {
    excitation_electrode_[i].getRect(&tmp_x, &tmp_y, &tmp_w, &tmp_h);
    excitation_electrode_[i].setRect(tmp_y*factor + offset_x1, tmp_x*factor + offset_y1, tmp_w, tmp_h);
    painter.setBrush(QBrush(Qt::gray, Qt::SolidPattern));
    painter.drawEllipse(excitation_electrode_[i]);
    painter.setFont(QFont("Arial", BioTacGui::font_size_2_));
    excitation_electrode_[i].setRect(tmp_y*factor + offset_x4, tmp_x*factor + offset_y4, tmp_w, tmp_h);
    // append X in front of the electrode number
    painter.drawText(excitation_electrode_[i], "X" + QString::number(i+1));
  }
}


void BioTacGui::convertRawToRgb(qreal value)
{
  // RGB default values (i.e. blue at the beginning)
  red_ = 0;
  green_ = 0;
  blue_ = 255;

  // thresholds to convert electrode impedance to RGB values
  int e0 = 0;
  int e1 = 1000;
  int e2 = 2000;
  int e3 = 3000;
  int e4 = 4095;

  if (value >= e0 && value < e1)
  {
    red_ = 255;
    green_ = 255*((value-e0)/(e1-e0));
    blue_ = 0;
  }
  else if (value >= e1 && value < e2)
  {
    red_ = 255*((e2-value)/(e2-e1));
    green_ = 255;
    blue_ = 0;
  }
  else if (value >= e2 && value < e3)
  {
    red_ = 0;
    green_ = 255;
    blue_ = 255*((value-e2)/(e3-e2));
  }
  else if (value >= e3 && value <= e4)
  {
    red_ = 0;
    green_ = 255*((e4-value)/(e4-e3));
    blue_ = 255;
  }
}

void BioTacGui::on_startButton_clicked()
{
  bool qstatus = true;
  qstatus = qnode_.init();
  
  if (qstatus == true)
  {
    std::cout << "Starting the node" << std::endl;
  }  
  else
  {  
    std::cout << "SOMETHING WRONG!" << std:: endl;
  }
}

void BioTacGui::on_closeButton_clicked()
{
  close();
}

void BioTacGui::updateGui()
{
  update();
}

void BioTacGui::closeGui()
{
  QObject::disconnect(&qnode_, SIGNAL(dataUpdated()), this, SLOT(updateGui()));
  delete [] data_ptr_;
  delete ui_;
}

}  // namespace st_biotac_gui
